/***************************************************************************\
 * Copyright (C) by Keio University
 * RigidTransformationEstimation.cpp created in 10 2012.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * RigidTransformationEstimation.cpp is part of the HVRL Engine Project.
 *
 * The HVRL Engine Project is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Project is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include "hvrl/tools/RigidTransformationEstimation.hpp"
#include "hvrl/common/Common.hpp"

namespace hvrl {

namespace RigidTransformationEstimation {

bool leastSquaresEstimation(const cv::Mat& srcA, const cv::Mat& srcB,
		cv::Mat& R, cv::Vec3f& t) {

	// some preliminary checks
	if (srcA.rows != srcB.rows || srcA.cols != srcB.cols) {
		hvrl::Log::add().error(
				"RigidTransformationEstimation::leastSquaresEstimation",
				"Input matrices have to be defined with the same size");
		return false;
	}

	float nbelements = 1.0f / float(srcA.cols);


	// Compute the mean vectors
	cv::Mat meanA(3, 1, CV_32F, 0.0f);
	cv::Mat meanB(3, 1, CV_32F, 0.0f);
	for (int i = 0; i < srcA.cols; ++i) {
		meanA.at<float>(0, 0) += srcA.at<float>(0, i);
		meanA.at<float>(1, 0) += srcA.at<float>(1, i);
		meanA.at<float>(2, 0) += srcA.at<float>(2, i);
		meanB.at<float>(0, 0) += srcB.at<float>(0, i);
		meanB.at<float>(1, 0) += srcB.at<float>(1, i);
		meanB.at<float>(2, 0) += srcB.at<float>(2, i);
	}
	meanA *= nbelements;
	meanB *= nbelements;

	/*// Compute the variance around the mean vectors
	float varianceA = 0.0f;
	cv::Mat srcAc (srcA.size(),srcA.type());
	cv::Mat srcBc (srcB.size(),srcB.type());
	for (unsigned int i = 0; i < srcA.cols; ++i) {
		varianceA += std::pow((srcA.at<float>(0, i) - meanA.at<float>(0, 0)),2) + std::pow((srcA.at<float>(1, i) - meanA.at<float>(1, 0)),2) + std::pow((srcA.at<float>(2, i) - meanA.at<float>(2, 0)),2);

		srcAc.at<float>(0, i) = srcA.at<float>(0, i) - meanA.at<float>(0, 0);
		srcAc.at<float>(1, i) = srcA.at<float>(1, i) - meanA.at<float>(1, 0);
		srcAc.at<float>(2, i) = srcA.at<float>(2, i) - meanA.at<float>(2, 0);

		srcBc.at<float>(0, i) = srcB.at<float>(0, i) - meanB.at<float>(0, 0);
		srcBc.at<float>(1, i) = srcB.at<float>(1, i) - meanB.at<float>(1, 0);
		srcBc.at<float>(2, i) = srcB.at<float>(2, i) - meanB.at<float>(2, 0);
	}
	varianceA *= nbelements;

	// Compute the covariance matrix of srcA and srcB
	cv::Mat covariance = srcAc * srcBc.t();
	covariance *= nbelements;

	// Compute the singular value decomposition of the covariance matrix
	cv::Mat D, U, Vt;
	cv::SVD::compute(covariance, D, U, Vt); // D is assumed as sorted*/

	cv::Mat A(3,1,CV_32F);
	cv::Mat B(1,3,CV_32F);
	cv::Mat H(3,3,CV_32F,0.0f);

	for (int i = 0; i < srcA.cols; ++i) {
		A.at<float>(0,0) = srcA.at<float>(0, i) - meanA.at<float>(0, 0);
		A.at<float>(1,0) = srcA.at<float>(1, i) - meanA.at<float>(1, 0);
		A.at<float>(2,0) = srcA.at<float>(2, i) - meanA.at<float>(2, 0);

		B.at<float>(0,0) = srcB.at<float>(0, i) - meanB.at<float>(0, 0);
		B.at<float>(0,1) = srcB.at<float>(1, i) - meanB.at<float>(1, 0);
		B.at<float>(0,2) = srcB.at<float>(2, i) - meanB.at<float>(2, 0);

		H += A*B;
	}

	cv::Mat D, U, Vt;
	cv::SVD::compute(H, D, U, Vt, cv::SVD::FULL_UV);

	//std::cout <<  Vt << std::endl;
	cv::Mat V = Vt.t();

	if(cv::determinant(V) * cv::determinant(U) < 0){
			V.at<float>(0, 2) *= -1;
			V.at<float>(1, 2) *= -1;
			V.at<float>(2, 2) *= -1;
	}
	cv::Mat X = V * U.t();

	float determinant = cv::determinant(X);

	if(determinant > 0){
		X.copyTo(R);
		cv::Mat t_ = meanB - X * meanA;

		t[0] = t_.at<float>(0,0);
		t[1] = t_.at<float>(1,0);
		t[2] = t_.at<float>(2,0);
		/*for (unsigned int i = 0; i < srcA.cols; ++i) {
			A.at<float>(0,0) = srcA.at<float>(0, i);
			A.at<float>(1,0) = srcA.at<float>(1, i);
			A.at<float>(2,0) = srcA.at<float>(2, i);

			B.at<float>(0,0) = srcB.at<float>(0, i);
			B.at<float>(0,1) = srcB.at<float>(1, i);
			B.at<float>(0,2) = srcB.at<float>(2, i) ;

			H += A*B;
		}*/
		return true;
	}
	return false;


	/*// Estimate S
	cv::Mat S = cv::Mat::eye(D.rows, D.rows, D.type());
	float determinant = cv::determinant(covariance);
	if (determinant < 0) {
		S.at<float>(D.rows - 1, D.rows - 1) = -1.0f;
	}

	float trace = 0.0f;
	int rank = srcA.rows;
	for (unsigned int i = 0; i < D.rows; ++i) {
		trace += S.at<float>(i, i) * D.at<float>(i, 0);
		if (D.at<float>(i, 0) == 0)
			rank--;
	}

	float scale = trace / varianceA;

	if (rank == srcA.rows - 1) {
		S = cv::Mat::eye(D.rows, D.rows, D.type());
		if (cv::determinant(U) * cv::determinant(Vt.t()) == -1) {
			S.at<float>(D.rows - 1, D.rows - 1) = -1.0f;
		}
	}
	cv::Mat R_ = U * S * Vt;
	R = R_;

	cv::Mat t_ = meanB - scale * R_ * meanA;

	t[0] = t_.at<float>(0,0);
	t[1] = t_.at<float>(1,0);
	t[2] = t_.at<float>(2,0);

	//R *= scale;*/

	return true;
}

}

}
